﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Runtime.InteropServices;
using System.Windows.Forms;
using System.Threading;
using Multimedia;
using System.Timers;

namespace Trio.Motor
{
    public class Motor
    {
        TrioP600 TrioP600 = new TrioP600();
        public double speed_Aim;        //方向（1顺时针 0逆时针）
        public int DIR = 0;        //方向通道


        double error = 0;
        double derror = 0;

        int outputV1 = 1000;
        double lasterror = 0;
        public double outputV = 0;
        double zerror = 0;
        public int BRDirection = 1;//1：正向，2：反向
        public int RXDirection = 1;//1:绕线，2：倒线
        double kp, ki, kd;//pid控制的三个系数

        double[] nCNTValue_old = new double[4];//上一时刻的编码器数值
        public double _velocity = 0, _velocity1 = 0, distance = 0;
        double WheelDiameter = 0.16;

        public Motor()
        {

        }
        ~Motor()
        {
        }

        //yyb
        private bool OpenDevice()
        {
            if (TrioP600.Connect())
            {
                _capturetimer_timer.Interval = 29; //这个时间单位毫秒,比如10秒，就写10000      19
                _capturetimer_timer.Elapsed += new ElapsedEventHandler(_capturetimer_timer_Elapsed);

                BeginEncoder();
                return true;
            }
            else
            {
                return false;
            }
        }
        public void SetZeroEp()
        {
            Encoder_Reset();
        }

        double[] nCNTValue = new double[4];  //编码器4个通道数值，只用第0通道，电压
        //yyb
        private void BeginEncoder()
        {
            Encoder_Reset();
            _capturetimer_timer.Start();
        }
        //yyb
        public List<double> distance_list = new List<double>();

        DateTime dt_last;
        TimeSpan Time;
        private void _capturetimer_timer_Elapsed(object sender, EventArgs e)
        {
            nCNTValue_old[0] = nCNTValue[0];
            nCNTValue[0] = Math.Abs(TrioP600.GetEncoder(4));

            if (nCNTValue[0] < nCNTValue_old[0] * 2)
            {
                //计算距离2147483648
                distance = ((nCNTValue[0]) / 8000.0 * WheelDiameter * 3.1415926);   //nCNTValue_old[0]
                distance_list.Add(distance);

                DateTime dt_now = DateTime.Now;
                Time = dt_now.Subtract(dt_last);

                //计算速度
                //_velocity = Math.Abs(nCNTValue_old[0] - nCNTValue[0]) / 8000.0 * WheelDiameter * 3.1415926 / _capturetimer_timer.Interval * 1000;
                _velocity = Math.Abs(nCNTValue_old[0] - nCNTValue[0]) / 8000.0 * WheelDiameter * 3.1415926 / Time.TotalMilliseconds * 1000;

                dt_last = dt_now;
            }
        }

        //yyb
        private void Encoder_Reset()
        {
            _velocity = 0;
            _velocity1 = 0;
            distance = 0;
            nCNTValue_old[0] = 0;
            nCNTValue[0] = 0;
            distance_list.Clear();
            TrioP600.InitMpos();
        }
        //yyb 获取速度
        public double GetVelocity()
        {
            return _velocity;
        }
        public bool Init()
        {
            if (OpenDevice())
            {
                Thread.Sleep(50);
                return true;
            }
            else
            {
                return false;
            }
        }

        public void End()
        {
            if (_sdCtlTimer != null)
            {
                _sdCtlTimer.Stop();
            }

            _capturetimer_timer.Stop();
        }
        public void stop()
        {
            setclutch(0, 0);

            distance_list.Clear();
            if (p != null)
            {
                if (p.IsAlive)
                {
                    p.Abort();
                }
            }

            InstStopRoll();
            TrioP600.StopMove();
        }
        //yyb
        public void setclutch(double a, double b)//切换磁粉离合器 输入力矩     
        {
            if (a > 0)
            {
                TrioP600.SetTension(0, 12);//N
                TrioP600.SetTension(8, a);
            }

            else if (b > 0)
            {
                TrioP600.SetTension(0, b);
                TrioP600.SetTension(8, 12);   //1.3nm
            }
            else
            {
                TrioP600.SetTension(0, 1);
                TrioP600.SetTension(8, 1);
            }
        }

        System.Timers.Timer _capturetimer_timer = new System.Timers.Timer();
        Thread p;
        public void start()
        {
            if (DIR == 1)
            {
                TrioP600.InitMotor(3);
            }
            else
            {
                TrioP600.InitMotor(10);
            }

            p = new Thread(new ThreadStart(startrotate));
            p.IsBackground = true;
            p.Start();
        }
        private Multimedia.Timer _sdCtlTimer;    //恒线速，速度控制定时器
        private void startrotate()
        {
            kp = 0.05;  //0.5
            ki = 0.03;   //0.03
            kd = 0.002;   //0.002
            lasterror = 0;
            error = 0;
            derror = 0;
            zerror = 0;
            outputV = 0;
            outputV1 = 0;

            _sdCtlTimer = new Multimedia.Timer();
            _sdCtlTimer.Mode = TimerMode.Periodic;
            _sdCtlTimer.Period = 200;
            _sdCtlTimer.Resolution = 1;
            _sdCtlTimer.Tick += _sdCtlTimer_Tick;

            _sdCtlTimer.Start();//开始速度控制调节
        }
        //速度调节控制
        int num_speed = 0;
        private void _sdCtlTimer_Tick(object sender, EventArgs e)
        {
            double speed_Cur = 0;
            speed_Cur = GetVelocity();

            if (speed_Cur < 0.01)
            {
                num_speed++;
                if (num_speed > 20)
                {
                    InstStopRoll();
                    MessageBox.Show("速度过慢，请检查是否卡带。");
                    num_speed = 0;
                }
            }

            error = speed_Aim - speed_Cur;

            if (error > speed_Aim * 0.05)
            {
                derror = (error - lasterror);
                zerror = derror - zerror;
                outputV = ki * error + kp * derror + kd * (error - 2 * derror + zerror) + outputV;
                if ((int)(outputV / (3.1415926 * WheelDiameter) * 8000) >= 0)
                {
                    outputV1 = (int)(outputV / (3.1415926 * WheelDiameter) * 8000);
                    if (outputV1 > 2000)
                    {
                        outputV1 = 2000;
                    }
                }
            }
            else
            {
                if (error < 0)
                {
                    outputV1--;
                }
            }

            if (DIR == 1)
            {
                StartRoll(3, outputV1);//右
            }
            else
            {
                StartRoll(10, outputV1);
            }
        }
        public void InstStopRoll()
        {
            if (_sdCtlTimer != null)
            {
                _sdCtlTimer.Stop();
            }

            if (DIR == 1)
            {
                StartRoll(3, 0);//x轴正转
            }
            else
            {
                StartRoll(10, 0);
            }
            TrioP600.StopMove();
        }
        public void StartRoll(int axis, Double speed)
        {
            TrioP600.CtrlSpeed(axis, speed);
        }
    }
}
